#include "Parse2DDicomImage.h"

#include <opencv2/features2d.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <QStandardPaths>
#include <QDir>
#include <QDebug>
#include <vtkSmartPointer.h>
#include <vtkDICOMImageReader.h>
#include <vtkImageCast.h>
#include <vtkImageData.h>

#include <memath/MeCvMath.h>
#include <memath/memath.h>

using namespace cv;
using namespace std;

double distance2(Point2d& p1, Point2d& p2)
{
    return pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2);
}

double distance2(Point& p1, Point& p2)
{
    return pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2);
}

double realDistance2(const KeyPoint& p1, const KeyPoint& p2)
{
    return sqrt(pow(p1.pt.x - p2.pt.x, 2) + pow(p1.pt.y - p2.pt.y, 2));
}

double realDistance2(Point2d& p1, Point2d& p2)
{
    return sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2));
}

double realDistance2(Point2f& p1, Point2f& p2)
{
    return sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2));
}

double realDistance2(CircleCenter& p1, CircleCenter& p2)
{
    return sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2));
}

double distance2(CircleCenter& p1, CircleCenter& p2)
{
    return pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2);
}

bool inAntiClockOrder(Point2d& p1, Point2d& p2, const Point2d& p3)
{
    return (p2.x - p1.x) * (p3.y - p2.y) - (p2.y - p1.y) * (p3.x - p2.x) > 0;
}

bool inAntiClockOrder(CircleCenter& p1, CircleCenter& p2, CircleCenter& p3)
{
    return (p2.x - p1.x) * (p3.y - p2.y) - (p2.y - p1.y) * (p3.x - p2.x) > 0;
}

double getAngle(Point2d& pt0, Point2d& pt1, Point2d& pt2)
{
    double dx1 = pt1.x - pt0.x;
    double dy1 = pt1.y - pt0.y;
    double dx2 = pt2.x - pt0.x;
    double dy2 = pt2.y - pt0.y;

    double angle_line = (dx1*dx2 + dy1 * dy2) / sqrt((dx1*dx1 + dy1 * dy1)*(dx2*dx2 + dy2 * dy2) + 1e-10);

    if (angle_line >= 1.0){
        return 0;
    } else if (angle_line <= -1.0) {
        return 180;
    }
    return acos(angle_line) * 180 / M_PI;
}

bool inLine(Point2d& p1, Point2d& p2, Point2d& p3)
{
    // (y3−y1)(x2−x1)−(y2−y1)(x3−x1)=0
    double angled = getAngle(p1, p2, p3);
    if(angled < 1.0){
        return true;
    }
    return false;
}
bool inLine(Point2d& p1, Point2d& p2, Point2d& p3,double& angle)
{
    // (y3−y1)(x2−x1)−(y2−y1)(x3−x1)=0
    angle = getAngle(p1, p2, p3);

    if(angle < 1.0){
        return true;
    }
    return false;
}
double distanceToLine(Point2d p, Point2d a, Point2d b)
{
    //求直线方程
    double A = a.y - b.y;
    double B = b.x - a.x;
    double C = a.x * b.y - a.y * b.x;

    //代入点到直线距离公式
    return (fabs(A * p.x + B * p.y + C)) / (sqrt(A * A + B * B));
}

bool cmpCircleCenterDefault(const CircleCenter &a, const CircleCenter &b)
{
    return a.r < b.r;
}
bool cmpPiontDistanceDefault(const PointDistance &a, const PointDistance &b)
{
    return a.distance < b.distance;
}
bool rectIntersectCircle(RotatedRect rect,Point2d a,double r){


    Point2f vertices[4];      //定义4个点的数组
    rect.points(vertices);   //将四个点存储到vertices数组中
    for (int i = 0; i < 4; i++){
        // 注意Scala中存储顺序 BGR
        if(distanceToLine(a,vertices[i], vertices[(i+1)%4])<r){
            return true;
        }
    }
    return false;
}

bool isSameDistance(double a, double b){
    return (a > b * 0.8) && (a < b * 1.2);
}
int signnum(double a){
    if (a > 0.0){
        return 1;
    }
    if(a<0.0){
        return -1;
    }
    return 0;
}
/**
  找到两个点的中点
 * @brief getMidPoint
 * @param pt1
 * @param pt2
 * @param ptM
 * @return
 */
bool getMidPoint(Point2d& pt1, Point2d& pt2, Point2d& ptM)
{
    double x = (pt1.x + pt2.x)/2;
    double y = (pt1.y + pt2.y)/2;
    ptM.x=x;
    ptM.y=y;
    return true;
}
/**
 * 在图上以ptCenter为圆心，顺时针旋转点p，旋转角度为angle（0-360），返回旋转后的点坐标
 * @brief rotatePoint
 * @param p
 * @param ptCenter
 * @param angle
 * @return
 */
Point2d rotatePoint(const Point2d p,const Point2d ptCenter,const double angle){
    double x1=p.x;
    double y1=p.y;
    double x2=ptCenter.x;
    double y2=ptCenter.y;
    double x= (x1 - x2)*cos(M_PI / 180.0 * angle) - (y1 - y2)*sin(M_PI / 180.0 * angle) + x2 ;
    double y= (x1 - x2)*sin(M_PI / 180.0 * angle) + (y1 - y2)*cos(M_PI / 180.0 * angle) + y2 ;
    return Point2d(x,y);
}

Parse2DDicomImage::Parse2DDicomImage()
{
}

QSharedPointer<DVImage> Parse2DDicomImage::getDicomImageData(QString& filename, bool toLog)
{
    vtkSmartPointer<vtkDICOMImageReader> reader = vtkSmartPointer<vtkDICOMImageReader>::New();
    if (!reader->CanReadFile(filename.toStdString().c_str())) {
        qDebug() << "[getDicomImageData] Unable to read file:" << filename;
        return QSharedPointer<DVImage>();
    }

    reader->SetFileName(filename.toStdString().c_str());
    reader->Update();

    // 图像的基本信息
    int dims[3];
    reader->GetOutput()->GetDimensions(dims);
    if (toLog) {
        qDebug() << "dimensions of [" << filename << "]:" << dims[0] << dims[1] << dims[2];
    }

    vtkImageData* imageData = reader->GetOutput();
    double scalarRange[2];
    imageData->GetScalarRange(scalarRange);
    if (toLog) {
        int scalarSize = imageData->GetScalarSize();
        int scalarType = imageData->GetScalarType();
        double typeMax = imageData->GetScalarTypeMax();
        double typeMin = imageData->GetScalarTypeMin();
        qDebug() << "scalarSize:" << scalarSize << "scalarType:" << scalarType << "typeMin:" << typeMin << "typeMax:" << typeMax;
        qDebug() << "scalarRange:" << scalarRange[0] << scalarRange[1];
    }

    ushort *pData = static_cast<ushort*>(imageData->GetScalarPointer());
    ushort minValue = ushort(scalarRange[0]), maxValue = ushort(scalarRange[1]);

    // 返回一个RGBA的QImage用于显示，以及一个灰色gray的Mat用于获取铁球的位置
    ushort range = maxValue - minValue;
    // 有时maxValue == minValue, 此时range应该为1
    if (range == 0) range = 1;

    QSharedPointer<DVImage> dvImage = QSharedPointer<DVImage>(new DVImage);

    Mat& grayMat =  dvImage->cvMat;
    grayMat.create(dims[1], dims[0], CV_8UC1);

    dvImage->qImage = QSharedPointer<QImage>(new QImage(dims[0], dims[1], QImage::Format_RGBA8888));
    QImage * resultImage = dvImage->qImage.data();
    for (int y = 0; y < dims[1]; y++) {
        uchar *line = resultImage->scanLine(y);
        uchar *matLine = grayMat.ptr(y);
        int loffset = dims[0] * (dims[1] - 1 - y);
        for (int x = 0; x < dims[0]; x++) {
            uint value = uint((pData[loffset + x] - minValue) << 8) / range;
            // 有时value会为256，转成uchar时就变成0了，这样就会在图片上显示成黑的斑点
            if (value > 255) value = 255;
            uchar gray = uchar(value);
            *(line+0) = gray;
            *(line+1) = gray;
            *(line+2) = gray;
            *(line+3) = 255;
            line += 4;
            matLine[x] = gray;
        }
    }

    return dvImage;
}

class Inner024 {
public:
    Inner024(vector<Point2d>& _points, ulong i0, ulong i2, ulong i4):
        points(_points) {
        data.push_back(i0);
        data.push_back(i2);
        data.push_back(i4);
    }
    void dump() {
        qDebug() << "Inner024:" << data[0] << data[1] << data[2];
    }
    vector<Point2d>& points;
    vector<ulong> data;
    Point2d& getPt(int ptIndex) const {
        switch(ptIndex) {
        case 0: return points[data[0]];
        case 2: return points[data[1]];
        default:
        case 4: return points[data[2]];
        }
    }
};

class Inner13 {
public:
    Inner13(vector<Point2d>& _points, ulong i1, ulong i3,double _errInner1, double _errInner3):
        points(_points), errInner1(_errInner1),errInner3(_errInner3){
        data.push_back(i1);
        data.push_back(i3);
    }
    vector<Point2d>& points;
    vector<ulong> data;
    double errInner1 = 0;
    double errInner3 = 0;
    Point2d& getPt(int ptIndex) const {
        switch(ptIndex) {
        case 1: return points[data[0]];
        default:
        case 3: return points[data[1]];
        }
    }
};

class Outer015 {
public:
    Outer015(vector<Point2d>& _points, ulong i0, ulong i1, ulong i5,double _errOut015):
        points(_points), errOut015(_errOut015) {
        data.push_back(i0);
        data.push_back(i1);
        data.push_back(i5);
    }
    vector<Point2d>& points;
    vector<ulong> data;
    double errOut015 = 0;
    Point2d& getPt(int ptIndex) const {
        switch(ptIndex) {
        case 0: return points[data[0]];
        case 1: return points[data[1]];
        default:
        case 5: return points[data[2]];
        }
    }
};

class Outer024 {
public:
    Outer024(vector<Point2d>& _points, ulong i0, ulong i2, ulong i4, double _errOut024):
        points(_points), errOut024(_errOut024) {
        data.push_back(i0);
        data.push_back(i2);
        data.push_back(i4);
    }
    vector<Point2d>& points;
    vector<ulong> data;
    double errOut024 = 0;
    Point2d& getPt(int ptIndex) const {
        switch(ptIndex) {
        case 0: return points[data[0]];
        case 2: return points[data[1]];
        default:
        case 4: return points[data[2]];
        }
    }
};

class PointsGroup {
public:
    PointsGroup(){}
    PointsGroup(vector<KeyPoint> _outerPionts, vector<KeyPoint> _innerPionts):
    outerPionts (_outerPionts),innerPionts(_innerPionts){

    }
    vector<KeyPoint> outerPionts;
    vector<KeyPoint> innerPionts;
    double errInner1;
    double errInner3;
    double errOuter015;
    double errOuter024;
    double errOuter3;
    double errOuter4;
    bool hasOut6;
};

bool compareGroup(PointsGroup group1, PointsGroup group2){
    return group1.errOuter3 < group2.errOuter3;
#if 0
    int i = 0;
    i += signnum(group1.errInner1 - group2.errInner1);
    i += signnum(group1.errInner3 - group2.errInner3);
    i += signnum(group1.errOuter015 - group2.errOuter015);
    i += signnum(group1.errOuter024 - group2.errOuter024);
    i += signnum(group1.errOuter3 - group2.errOuter3);
    qDebug("i:%d,(%fx%f),(%fx%f),(%fx%f),(%fx%f),(%fx%f)", i,
           group1.errInner1, group2.errInner1,
           group1.errInner3, group2.errInner3,
           group1.errOuter015, group2.errOuter015,
           group1.errOuter024, group2.errOuter024,
           group1.errOuter3, group2.errOuter3);
    return i <= 0;
#endif
}

class YDPointArea {
public:
    vector<Point2d>& points;
    vector<ulong> data;
};

// 得到所有的Inner上的0,2,4点
vector<Inner024> getAllInner024(vector<Point2d>& points)
{
    vector<Inner024> result;

    //求出所有点的外接矩形
    vector<Point2f> points2f;
    foreach(Point2d pt, points) points2f.push_back({float(pt.x), float(pt.y)});

    RotatedRect rect = minAreaRect(points2f);
    double maxHeight = double(max(rect.size.width, rect.size.height));

    vector<CircleCenter> findPoints;
    ulong count = points.size();

    for (ulong i = 0; i < count-1; i++) {
        for (ulong j = i+1; j < count; j++) {
            double distance = realDistance2(points[i], points[j]);
            if (distance < maxHeight/5.0) {
                for (ulong k = 0; k < count; k++) {
                    if(k != i && k != j){
                        double distanceik = realDistance2(points[i], points[k]);
                        double distancejk = realDistance2(points[j], points[k]);
                        if (distanceik > distance * 4 && distancejk > distance * 4) {
                            ulong index0 = i, index4 = j, index2 = k;
                            if (distancejk > distanceik) {
                                swap(index0, index4);
                            }
                            if (inLine(points[index2], points[index4], points[index0])) {
                                result.push_back({points, index0, index2, index4});
                            }
                        }
                    }
                }
            }
        }
    }
    return result;
}

Mat getOuterTransform0123(Point2d& pt0, Point2d& pt1, Point2d& pt2, const Point2d& pt3)
{
    vector<Point2f> ptSide= {
        {7.0f, 113.65f},
        {0.0, 180.65f},
        {74.0f, 180.65f},
        {67.0f, 113.65f}

    };
    vector<Point2f> ptTrans = {pt0, pt1, pt2, pt3};
    return getPerspectiveTransform(ptSide, ptTrans);
}

Mat getInnerTransform0123(Point2d& pt0, const Point2d& pt1, Point2d& pt2, const Point2d& pt3)
{    
    vector<Point2f> ptSide = {
        {73.77f, 143.65f},
        {37.0f, 180.42f},
        {0.23f, 143.65f},
        {37.0f, 106.88f}

    };
    vector<Point2f> ptTrans = {pt0, pt1, pt2, pt3};
    return getPerspectiveTransform(ptSide, ptTrans);
}
/**
 * @brief getPointErr
 * @param transform 投影矩阵
 * @param ptSideM 投影前的点
 * @param ptM 实际的点
 * @return
 */
double getPointErr(Mat& transform,Point2d& ptSideM, Point2d& ptM)
{
    vector<Point2f> ptSideVector, ptTransVector;
    ptSideVector.push_back(ptSideM);

    //计算投影后的点
    perspectiveTransform(ptSideVector, ptTransVector, transform);

    Point2d ptTransOutM(ptTransVector[0]);

    return realDistance2(ptTransOutM, ptM);
}
double getCenterPointMapErr(Mat& transform, Point2d& ptC, Point2d& debugPt)
{
    vector<Point2f> ptSideVector, ptTransVector;
    ptSideVector.push_back({37.0f, 142.15f});

    //计算投影后的点
    perspectiveTransform(ptSideVector, ptTransVector, transform);

    Point2d ptTransOutC(ptTransVector[0]);
    debugPt.x = ptTransOutC.x;
    debugPt.y = ptTransOutC.y;
    return realDistance2(ptTransOutC, ptC);
}

double getInnerPt4MapErr(Mat& transform, Point2d& pt4)
{
    Point2f ptSideOut5(63.77f, 143.65f);
    vector<Point2f> ptSideVector, ptTransVector;
    ptSideVector.push_back(ptSideOut5);

    //计算投影后的点
    perspectiveTransform( ptSideVector, ptTransVector, transform);

    Point2d ptTransOut5(ptTransVector[0]);
    return realDistance2(ptTransOut5, pt4);
}

vector<Inner13> getAllInner13(vector<Point2d>& points, const Inner024& inner024, vector<ulong>& antiClockIndexes, vector<ulong>& clockIndexes)
{
    Point2d& pt0 = inner024.getPt(0);
    Point2d& pt2 = inner024.getPt(2);
    Point2d& pt4 = inner024.getPt(4);
    Point2d  ptCenter;
    getMidPoint(pt0, pt2, ptCenter);
    //分别让0，2绕中心旋转90度，找到理论上1，3的位置
    Point2d calPt1 = rotatePoint(pt0,ptCenter,90);
    Point2d calPt3 = rotatePoint(pt2,ptCenter,90);
    double halfDistance02 = realDistance2(pt0, pt2) / 2;
    vector<ulong> vectorClockOrderPiontsDist, vectorAntiClockOrderPiontsDist;
    for (ulong i = 0; i < points.size(); i++) {
        if (i == inner024.data[0] || i == inner024.data[1] || i == inner024.data[2])
            continue;
        Point2d& pt = points[i];
        double distance = distanceToLine(pt, pt0, pt2);
        double distance0pt = realDistance2(pt0, pt);
        double distance2pt = realDistance2(pt2, pt);
        if (inAntiClockOrder(pt0, pt2, pt)) {
            antiClockIndexes.push_back(i);
            if (isSameDistance(distance, halfDistance02) && isSameDistance(distance0pt, distance2pt)) {
                vectorAntiClockOrderPiontsDist.push_back(i);
            }
        } else {
            clockIndexes.push_back(i);
            if (isSameDistance(distance, halfDistance02) && isSameDistance(distance0pt, distance2pt)) {
                vectorClockOrderPiontsDist.push_back(i);
            }
        }
    }

    double matchErr = 5.0;
    double minErr = matchErr * 2;
    vector<Inner13> result;
    foreach(ulong ptAntiClockIndex, vectorAntiClockOrderPiontsDist) {
        foreach(ulong ptClockIndex, vectorClockOrderPiontsDist) {
            const Point2d& ptAntiClock = points[ptAntiClockIndex];
            const Point2d& ptClock = points[ptClockIndex];
            Mat transform = getInnerTransform0123(pt0, ptAntiClock, pt2, ptClock);
            double err = getInnerPt4MapErr(transform, pt4);
            if (err < matchErr) {
                double errInner1 = realDistance2(points[ptClockIndex], calPt1);
                double errInner3 = realDistance2(points[ptAntiClockIndex], calPt3);
                if (err < minErr) {
                    minErr = err;
                    result.push_back({points, ptClockIndex, ptAntiClockIndex, errInner1, errInner3});
                }
            }
        }
    }

    // 有多个时取最后的一个最小的返回
    if (result.size() > 1) {
        return vector<Inner13>(1, result[result.size() - 1]);
    }
    return result;
}

vector<Outer015> getOuter015(ulong point05AreaIndex, ulong point1AreaIndex, vector<YDPointArea>& fourArea)
{
    vector<Outer015> result;

    vector<Point2d>& points = fourArea[0].points;
    ulong ptCountIn05Area = fourArea[point05AreaIndex].data.size();
    ulong ptCountIn1Area = fourArea[point1AreaIndex].data.size();
    for (ulong i = 0; i < ptCountIn05Area - 1; i++) {
        for (ulong j = i + 1; j < ptCountIn05Area; j++) {
            for (ulong k = 0; k < ptCountIn1Area; k++) {
                ulong reali = fourArea[point05AreaIndex].data[i];
                ulong realj = fourArea[point05AreaIndex].data[j];
                ulong realk = fourArea[point1AreaIndex].data[k];
                Point2d& pt0 = points[reali];
                Point2d& pt5 = points[realj];
                Point2d& pt1 = points[realk];
                double angle;
                if (inLine(pt1, pt0, pt5,angle)) {
                    if (realDistance2(pt0, pt1) > realDistance2(pt5, pt1))
                        result.push_back({fourArea[0].points, reali, realk, realj,angle});
                    else
                        result.push_back({fourArea[0].points, realj, realk, reali,angle});
                }
            }
        }
    }
    return result;
}

vector<Outer024> getOuter024(const Outer015& outer015, ulong point045AreaIndex, vector<YDPointArea>& fourArea)
{
    vector<Outer024> result;

    // 如果045点在area0(index=0), 那么2点area2
    // 如果045点在area1(index=1), 那么2点area3
    // 所以为index+2
    ulong point2AreaIndex = point045AreaIndex + 2;
    ulong ptCountIn2Area = fourArea[point2AreaIndex].data.size();
    ulong ptCountIn045Area = fourArea[point045AreaIndex].data.size();
    Point2d& pt0 = outer015.getPt(0);
    vector<Point2d>& points = outer015.points;
    for (ulong i = 0; i < ptCountIn045Area; i++) {
        ulong vi = fourArea[point045AreaIndex].data[i];
        if (vi == outer015.data[0] || vi == outer015.data[2]) continue;
        for (ulong j = 0; j < ptCountIn2Area; j++) {
            Point2d& pt4 = points[vi];
            Point2d& pt2 = points[fourArea[point2AreaIndex].data[j]];
            double angle;
            if (inLine(pt2, pt0, pt4, angle) && realDistance2(pt2,pt4) > realDistance2(pt2,pt0)) {
                // 4在0的外面
                // angle就是error值, TODO
                result.push_back({outer015.points, outer015.data[0], fourArea[point2AreaIndex].data[j], fourArea[point045AreaIndex].data[i], angle});

            }
        }
    }

    return result;
}
bool getOuter6(const Outer024& outer024,Point2d& out3, Mat transform, ulong point045AreaIndex, vector<YDPointArea>& fourArea, ulong& out6index)
{
    //vector<ulong> result;
    Point2d ptSideOut6(0.0, 113.65 );
    vector<Point2f> ptSideVector, ptTransVector;
    ptSideVector.push_back(ptSideOut6);
    //计算投影后的点
    perspectiveTransform( ptSideVector, ptTransVector, transform);
    Point2d ptTransOut6(ptTransVector[0]);

    bool foundOut6 = false;
    double minDistance=10;
    ulong ptCountIn045Area = fourArea[point045AreaIndex].data.size();
    Point2d& pt0 = outer024.getPt(0);
    vector<Point2d>& points = outer024.points;
    for (ulong i = 0; i < ptCountIn045Area; i++) {
        ulong vi = fourArea[point045AreaIndex].data[i];
        Point2d& pt6 = points[vi];

        if (vi == outer024.data[0] || vi == outer024.data[2] ) continue;
        if (inLine(pt6, pt0, out3)){
            double dis = realDistance2(ptTransOut6, pt6);
            if(dis<10 && dis<minDistance){
                foundOut6 = true;
                out6index= vi;
                minDistance = dis;
            }
        }

    }
    qDebug() << "errOuter6:" << minDistance;
    return foundOut6;
}

// 返回4个Area
// 24点对面的为0和1area
vector<YDPointArea> getFourArea(const Inner13& inner13, vector<ulong>& antiClockIndexes, vector<ulong>& clockIndexes)
{
    Point2d& pt1 = inner13.getPt(1);
    Point2d& pt3 = inner13.getPt(3);
    vector<Point2d>& points = inner13.points;
    vector<ulong> areaData0, areaData1, areaData2, areaData3;
    foreach(ulong i, clockIndexes) {
        if (i != inner13.data[0] && i != inner13.data[1]) {
            if (inAntiClockOrder(pt1, pt3, points[i])) {
                areaData3.push_back(i);
            } else {
                areaData0.push_back(i);
            }
        }
    }
    foreach(ulong i, antiClockIndexes) {
        if (i != inner13.data[0] && i != inner13.data[1]) {
            if (inAntiClockOrder(pt1, pt3, points[i])) {
                areaData2.push_back(i);
            } else {
                areaData1.push_back(i);
            }
        }
    }
    return {
        {points, areaData0},
        {points, areaData1},
        {points, areaData2},
        {points, areaData3}
    };
}

bool getLineCross(Point2d& pt0, Point2d& pt2, Point2d& pt1, const Point2d& pt3, Point2d& result)
{
    double a1 = pt0.y - pt2.y;
    double b1 = pt2.x - pt0.x;
    double c1 = -b1 * pt0.y - a1 * pt0.x;
    double a2 = pt1.y - pt3.y;
    double b2 = pt3.x - pt1.x;
    double c2 = -b2 * pt1.y - a2 * pt1.x;
    double d = (a1 * b2 - b1 * a2);
    if (fabs(d) > 1e-6) {
        result.y = (c1 * a2 - a1 * c2) / d;
        result.x = (c2 * b1 - c1 * b2) / d;
        return true;
    }
    return false;
}

// 4个Area中必须有1个Area中的点数>=3, 其他Area中点数>=1
// moreThanThreePointsIndexes中只能返回有0，或1，或0和1
bool matchAreaRequirements(vector<YDPointArea>& fourArea, vector<ulong>& areaContain045) {
    for (ulong i = 0; i <= 1; i++) {
        const YDPointArea& area = fourArea[i];
        if (area.data.size() < 1) {
            return false;
        }
        if (area.data.size() >= 3) {
            areaContain045.push_back(i);
        }
    }
    return areaContain045.size() > 0;
}

bool Parse2DDicomImage::detectRuler2DPoints(std::vector<cv::KeyPoint>& keyPoints, std::vector<cv::KeyPoint> &outerPoints, std::vector<cv::KeyPoint> &innerPoints, bool toLog, QString *imageOutputFolder)
{
    if(keyPoints.size() < 11) {
        return false;
    }
    Point2d ptMSide23(70.5, 147.15);
    Point2d ptMSide03(37.0, 113.65);
    Point2d ptOut4(2.05, 108.7);

    vector<Point2d> points;

    // 用于图像输出, debug
    cv::Mat img(2064, 2064, CV_8UC3, Vec3b(0, 0, 0));
    int pointIndex = 0;
    String str = "00";

    // 用不同的颜色来表示不同Area的点
    Vec3b color[] = {
        Vec3b(255, 0, 255),
        Vec3b(0, 255, 0),
        Vec3b(0, 0, 255),
        Vec3b(255, 255, 255)
    };

    // 把circleCenters转换成points
    foreach(const KeyPoint& cc, keyPoints) {
        points.push_back({double(cc.pt.x), double(cc.pt.y)});
        if (imageOutputFolder) {
            // for debuging
            Point pt = {int(cc.pt.x), int(cc.pt.y)};
            circle(img, pt, 40, Vec3b(50, 50, 50), -1);
            str[1] = char('0' + (pointIndex % 10));
            str[0] = char('0' + (pointIndex / 10));
            putText(img, str, Point(pt.x + 20, pt.y - 20), FONT_HERSHEY_DUPLEX, 2, Vec3b(255, 255, 255), 3);
            pointIndex++;
        }
    }
    vector<PointsGroup> pointsGroups;
    vector<Inner024> allInner024 = getAllInner024(points);
    int solutionIndex = 0;
    int solutionIndex2 = 0;
    foreach(const Inner024& inner024, allInner024) {
        vector<ulong> antiClockIndexes, clockIndexes;
        vector<Inner13> allInner13 = getAllInner13(points, inner024, antiClockIndexes, clockIndexes);
        solutionIndex++;
        foreach(const Inner13& inner13, allInner13) {
            vector<YDPointArea> fourArea = getFourArea(inner13, antiClockIndexes, clockIndexes);
            vector<ulong> possibleArea01;
            if (matchAreaRequirements(fourArea, possibleArea01)) {
                if (imageOutputFolder) {
                    line(img, Point(points[inner024.data[0]]), Point(points[inner024.data[1]]), Vec3b(255, 255, 0));
                    line(img, Point(points[inner024.data[1]]), Point(points[inner024.data[2]]), Vec3b(255, 128, 0));
                    line(img, Point(points[inner13.data[0]]), Point(points[inner13.data[1]]), Vec3b(255, 255, 0));
                    circle(img, Point(points[inner024.data[0]]), 20, Vec3b(255, 0, 0), 2);
                    circle(img, Point(points[inner024.data[2]]), 20, Vec3b(255, 0, 0), 2);
                    for (ulong i = 0; i < fourArea.size(); i++) {
                        for (ulong j = 0; j < fourArea[i].data.size(); j++) {
                            Point2d& pt = points[fourArea[i].data[j]];
                            circle(img, Point(pt), 20, color[i], 2);
                        }
                    }
                    QString fourarea = *imageOutputFolder + QString("/fourarea_solution%1").arg(solutionIndex) + ".png";
                    imwrite(fourarea.toStdString(), img);
                }

                if (toLog) qDebug() << "fourArea is match requirement-----------------------------";
                foreach(const ulong index0or1, possibleArea01) {
                    // 求Outer的015，如果045点在area0，那么1点在area1; 如果045点在area1，那么1点在area0;
                    vector<Outer015> allOuter015 = getOuter015(index0or1, 1 - index0or1, fourArea);
                    if (toLog) qDebug() << "index0or1:" << index0or1 << "allOuter015.size:" << allOuter015.size();
                    foreach(const Outer015& outer015, allOuter015) {
                        if (toLog) qDebug() << "outer015:" << outer015.data[0] << outer015.data[1] << outer015.data[2];
                        vector<Outer024> allOuter024 = getOuter024(outer015, index0or1, fourArea);
                        if (toLog) qDebug() << "allOuter024.size:" << allOuter024.size();
                        foreach(const Outer024& outer024, allOuter024) {
                            if (toLog) qDebug() << "outer024:" << outer024.data[0] << outer024.data[1] << outer024.data[2];
                            if (toLog) qDebug() << "should find last outer point in area:" << (3 - index0or1);
                            // 对Area中的每一个，求映射矩阵，再求基准点5和基准点C的映射点，然后求它们的偏差
                            foreach(ulong index, fourArea[3 - index0or1].data) {
                                if (toLog) qDebug() << "begin to handle last outer point with index:" << index;
                                Point2d& pt = points[index];
                                Point2d& pt0 = outer024.getPt(0);
                                Point2d& pt1 = outer015.getPt(1);
                                Point2d& pt2 = outer024.getPt(2);
                                Point2d& pt5 = outer015.getPt(5);
                                Mat transform = getOuterTransform0123(pt0, pt1, pt2, pt);
                                Point2d ptC;
                                Point2d ptM23,ptM03;
                                getMidPoint(pt2, pt, ptM23);
                                getMidPoint(pt0, pt, ptM03);
                                double err23 = getPointErr(transform, ptMSide23, ptM23);
                                double err03 = getPointErr(transform, ptMSide03, ptM03);
                                double errM = err23 + err03;
                                if (toLog) qDebug() << "err of getCenterPointMapErr:" << errM;
                                if (errM < 10.0) {
                                    if (imageOutputFolder) {
                                        cv::Mat img01235(2064, 2064, CV_8UC3, Vec3b(0, 0, 0));
                                        circle(img01235, Point(points[outer015.data[0]]), 16, Vec3b(0, 0, 255), -1);
                                        circle(img01235, Point(points[outer015.data[1]]), 16, Vec3b(0, 0, 255), -1);
                                        circle(img01235, Point(points[outer015.data[2]]), 16, Vec3b(0, 0, 255), -1);
                                        circle(img01235, Point(points[outer024.data[0]]), 8, Vec3b(255, 255, 255), -1);
                                        circle(img01235, Point(points[outer024.data[1]]), 8, Vec3b(255, 255, 255), -1);
                                        circle(img01235, Point(points[outer024.data[2]]), 8, Vec3b(255, 255, 255), -1);
                                        circle(img01235, Point(pt), 8, Vec3b(255, 255, 255), -1);
                                        circle(img01235, Point(pt0), 8, Vec3b(255, 255, 255), -1);
                                        circle(img01235, Point(pt1), 8, Vec3b(255, 255, 255), -1);
                                        circle(img01235, Point(pt2), 8, Vec3b(255, 255, 255), -1);
                                        circle(img01235, Point(pt5), 8, Vec3b(255, 255, 255), -1);
                                        char tmpBuffer[128];
                                        sprintf(tmpBuffer, "err: %f", errM);
                                        putText(img01235, tmpBuffer, Point(10, 40), FONT_HERSHEY_DUPLEX, 2, Vec3b(0, 0, 255), 2);
                                        putText(img01235, "3", Point(pt), FONT_HERSHEY_DUPLEX, 2, Vec3b(0, 0, 255), 3);
                                        putText(img01235, "0", Point(pt0), FONT_HERSHEY_DUPLEX, 2, Vec3b(0, 0, 255), 3);
                                        putText(img01235, "1", Point(pt1), FONT_HERSHEY_DUPLEX, 2, Vec3b(0, 0, 255), 3);
                                        putText(img01235, "2", Point(pt2), FONT_HERSHEY_DUPLEX, 2, Vec3b(0, 0, 255), 3);
                                        putText(img01235, "5", Point(pt5), FONT_HERSHEY_DUPLEX, 2, Vec3b(0, 0, 255), 3);
                                        QString img01235Str = *imageOutputFolder + QString("/img01235_%1").arg(solutionIndex2) + ".png";
                                        imwrite(img01235Str.toStdString(), img01235);
                                        solutionIndex2++;
                                    }
                                    //计算out4的投影偏差
                                     double err4 = getPointErr(transform, ptOut4, outer024.getPt(4));
                                    // OK to get one set
                                    vector<KeyPoint> outPoints,inPoints;
                                    outPoints.push_back(keyPoints[outer015.data[0]]);
                                    outPoints.push_back(keyPoints[outer015.data[1]]);
                                    outPoints.push_back(keyPoints[outer024.data[1]]);
                                    outPoints.push_back(keyPoints[index]);
                                    outPoints.push_back(keyPoints[outer024.data[2]]);
                                    outPoints.push_back(keyPoints[outer015.data[2]]);
                                    inPoints.push_back(keyPoints[inner024.data[0]]);
                                    inPoints.push_back(keyPoints[inner13.data[0]]);
                                    inPoints.push_back(keyPoints[inner024.data[1]]);
                                    inPoints.push_back(keyPoints[inner13.data[1]]);
                                    inPoints.push_back(keyPoints[inner024.data[2]]);
                                    if(inAntiClockOrder(outer015.getPt(0),outer015.getPt(1),outer024.getPt(2))){
                                        swap(inPoints[1],inPoints[3]);
                                    }
                                    ulong out6index;
                                    bool hasOut6 = getOuter6(outer024, pt,transform, index0or1,fourArea,out6index);
                                    if(hasOut6){
                                          outPoints.push_back(keyPoints[out6index]);
                                    }
                                    PointsGroup group(outPoints, inPoints);
                                    group.errInner1 = inner13.errInner1;
                                    group.errInner3 = inner13.errInner3;
                                    group.errOuter015 = outer015.errOut015;
                                    group.errOuter024 = outer024.errOut024;
                                    group.errOuter4 = err4;
                                    group.errOuter3 = errM;
                                    group.hasOut6 = hasOut6;
                                    pointsGroups.push_back(group);
                                    qDebug() << "errInner1:" <<group.errInner1 <<
                                                ", errInner3:" <<group.errInner3 <<
                                                ", errOuter015:"<<group.errOuter015 <<
                                                ", errOuter024:" <<group.errOuter024 <<
                                                 ", errOuter4:" <<group.errOuter4 <<
                                                 ", hasOut6:" <<group.hasOut6 <<
                                                ", errOuter3:"<<group.errOuter3;
                                }
                            }
                        }
                    }
                }
            }
        }
    }
    if(pointsGroups.empty()){
        return false;
    }
    sort(pointsGroups.begin(), pointsGroups.end(), compareGroup);
    PointsGroup& bestGroup = pointsGroups[0];
    //投影是利用out0123计算的，out4有可能不准确，重新比较out4的投影差
    if(pointsGroups.size() > 1){
        ulong bestIndex = 0;
        for(ulong i = 1; i < pointsGroups.size(); i++){
            if(pointsGroups[i].errOuter3 - pointsGroups[bestIndex].errOuter3 < 1e-6){
                //err3相同，表示是同一套out0123，比较errout4
                if (pointsGroups[i].errOuter4 < pointsGroups[bestIndex].errOuter4 ){
                    bestIndex = i;
                }
            }
        }
        bestGroup = pointsGroups[bestIndex];
    }

    outerPoints = bestGroup.outerPionts;
    innerPoints = bestGroup.innerPionts;
    return true;
}

#include <stack>
#include <vtkSmartPointer.h>
typedef struct _scanner {
    long m00;
    long m10;
    long m01;
    int width;
    int height;
    int src_step;
    // 简单计算最小最大XY
    int minx, maxx;
    int miny, maxy;
    char padding[4];
} scanner;

static void travel_from(const uchar* src, uchar* mask, scanner *ps, int sx, int sy,
                        std::vector<Point> & contours, uchar color) {
    stack<Point2i> stk;
    stk.push(Point2i(sx, sy));
    contours.push_back(Point2i(sx, sy));
    while (!stk.empty()) {
        Point2i& p = stk.top();
        stk.pop();
        sx = p.x; sy = p.y;
        int offset = ps->src_step * sy;
        int x, lx, rx;
        lx = sx;
        rx = sx;
        // 往左遍历
        for (x = sx - 1; x >= 0; x--) {
            if (src[offset + x] == color && !mask[offset + x]) {
                lx = x;
                mask[offset + x] = 255;
                ps->m00 += 1;
                ps->m10 += x;
                ps->m01 += sy;
            } else {
                break;
            }
        }
        // 往右遍历
        for (x = sx; x < ps->width; x++) {
            if (src[offset + x] == color && !mask[offset + x]) {
                rx = x;
                mask[offset + x] = 255;
                ps->m00 += 1;
                ps->m10 += x;
                ps->m01 += sy;
            } else {
                break;
            }
        }
        // 保存最左，最右
        contours.push_back(Point2i(lx, sy));
        contours.push_back(Point2i(rx, sy));
        if (lx < ps->minx) ps->minx = lx;
        if (rx > ps->maxx) ps->maxx = rx;
        if (sy > ps->maxy) ps->maxy = sy;
        int lastx1 = -2, lastx2 = -2;
        for (x = lx; x <= rx; x++) {
            if (sy > 0 && src[offset - ps->src_step + x] == color && !mask[offset - ps->src_step + x]) {
                if (x != lastx1 + 1) {
                    stk.push(Point2i(x, sy - 1));
                }
                lastx1 = x;
            }
            if (sy < ps->height-1 && src[offset + ps->src_step + x] == color && !mask[offset + ps->src_step + x]) {
                if (x != lastx2 + 1) {
                    stk.push(Point2i(x, sy + 1));
                }
                lastx2 = x;
            }
        }
    }
}

int fastBblobDetect(const Mat& _src, SimpleBlobDetector::Params &params, vector<KeyPoint>& keyPoints) {
    size_t src_step = _src.step;
    Size roi = _src.size();

    Mat tmp = Mat(_src.size(), _src.type());
    tmp.setTo(0);

    uchar* ptmp = tmp.ptr(); // 存放已经搜索过的
    const uchar* src = _src.ptr();
    int count = 0;
    scanner scer;
    scer.width = roi.width;
    scer.height = roi.height;
    scer.src_step = int(src_step);

    for(int i = 0; i < roi.height; i++, src += src_step, ptmp += src_step) {
        for (int j = 0; j < roi.width; j++) {
            // 白色 并且 不存在
            if (src[j] == params.blobColor && !ptmp[j]) {
                scer.m00 = 0;
                scer.m10 = 0;
                scer.m01 = 0;
                scer.minx = roi.width; scer.miny = i;
                scer.maxx = 0; scer.maxy = 0;

                float diameter = 0.0;
                vector<Point> contours;
                travel_from(_src.ptr(), tmp.ptr(), &scer, j, i, contours, params.blobColor);
                if (params.filterByArea) {
                    if (scer.m00 < params.minArea || scer.m00 >= params.maxArea) {
                        continue;
                    }
                    // 通过面积计算直径
                    diameter = float(2 * sqrt(double(scer.m00) / ME_PI));
                }

                if (params.filterByCircularity) {
                    // 简单的长宽比
                    float ratio = float(min((scer.maxx- scer.minx), (scer.maxy - scer.miny))) / max((scer.maxx- scer.minx), (scer.maxy - scer.miny));
                    if (ratio < params.minCircularity || ratio > params.maxCircularity) {
                        continue;
                    }
                }
                if (scer.m00 != 0) {
                    float cx = float(scer.m10) / float(scer.m00);
                    float cy = float(scer.m01) / float(scer.m00);
                    KeyPoint point(Point2f(cx, cy), diameter);
                    keyPoints.push_back(point);
                    count++;
                }
            }
        }
    }

    return count;
}

QSharedPointer<DVImage> Parse2DDicomImage::parseDicomImage(QString &filename, std::vector<KeyPoint> &keyPoints, bool toLog, QString *imageOutputFolder)
{
    cv::TickMeter tickMeter;
    tickMeter.start();

    QSharedPointer<DVImage> dvimg = getDicomImageData(filename, toLog);
    if (dvimg.isNull()) {
        return dvimg;
    }
    Mat &grayMat = dvimg->cvMat;

    SimpleBlobDetector::Params params;
    params.minArea = 36;
    params.maxArea = 1300; // 斑点的最大面积
    params.filterByArea = true;
    params.minCircularity = 0.8f;
    params.maxCircularity = 1.0f;
    params.filterByCircularity = true;
    params.blobColor = 0;

    Mat grayMatResized, mat1, mat2, mat3, mat4;
    std::vector<KeyPoint> keyPoints1, keyPoints2, keyPoints3, keyPoints4;
    // 缩小1/4后来搜索钢球位置，加快速度
    cv::resize(grayMat, grayMatResized, grayMat.size()/2);
    threshold(grayMatResized, mat1, 100, 255, THRESH_BINARY);
    threshold(grayMatResized, mat2, 90, 255, THRESH_BINARY);
    threshold(grayMatResized, mat3, 80, 255, THRESH_BINARY);
    threshold(grayMatResized, mat4, 50, 255, THRESH_BINARY);
    fastBblobDetect(mat1, params, keyPoints1);
    fastBblobDetect(mat2, params, keyPoints2);
    fastBblobDetect(mat3, params, keyPoints3);
    fastBblobDetect(mat4, params, keyPoints4);
    keyPoints.insert(keyPoints.begin(), keyPoints1.begin(), keyPoints1.end());

    // 把非重复的点加到keyPoints中
    foreach (const KeyPoint& pt2, keyPoints2) {
        bool toAdd = true;
        foreach (const KeyPoint& pt, keyPoints) {
            if (realDistance2(pt2, pt) < 10) {
                toAdd = false;
                break;
            }
        }
        if (toAdd) {
            keyPoints.push_back(pt2);
        }
    }
    foreach (const KeyPoint& pt3, keyPoints3) {
        bool toAdd = true;
        foreach (const KeyPoint& pt, keyPoints) {
            if (realDistance2(pt3, pt) < 10) {
                toAdd = false;
                break;
            }
        }
        if (toAdd) {
            keyPoints.push_back(pt3);
        }
    }

    // 把非重复的点加到keyPoints中
    foreach (const KeyPoint& pt4, keyPoints4) {
        bool toAdd = true;
        foreach (const KeyPoint& pt, keyPoints) {
            if (realDistance2(pt4, pt) < 10) {
                toAdd = false;
                break;
            }
        }
        if (toAdd) {
            keyPoints.push_back(pt4);
        }
    }

    tickMeter.stop();
    if (toLog) qDebug("time of threshold and fastBblobDetect in ms:%f", tickMeter.getTimeMilli());

    // 因为之前缩小了2倍，所以现在恢复要乘2
    for (vector<KeyPoint>::iterator iter = keyPoints.begin(); iter < keyPoints.end();) {
        iter->pt *= 2;
        iter->size *= 2;

        // roi 区域中更细检查，如果无法查到，那么删除该点
        bool bDel=false;
        cv::Rect clickRect;
        int w = dvimg->getQImageData()->width();
        int h = dvimg->getQImageData()->height();

        if (Parse2DDicomImage::getPointRect(w, h, iter->pt.x, iter->pt.y, clickRect, 100)) {
            // 获取点击的roi区域
            cv::Mat roiMat = grayMat(clickRect).clone();
            std::vector<cv::KeyPoint> pointsVect;
            if(!detectIronPointInRoi(roiMat, pointsVect, true)) {
                bDel = true;
            }
        }
        if (bDel) {
            keyPoints.erase(iter);
        } else {
            iter++;
        }
    }

    if(imageOutputFolder) {
        QString mat1Str = *imageOutputFolder + "/mat1.png";
        imwrite(mat1Str.toStdString(), mat1);
        QString mat2Str = *imageOutputFolder + "/mat2.png";
        imwrite(mat2Str.toStdString(), mat2);
        QString mat3Str = *imageOutputFolder + "/mat3.png";
        imwrite(mat3Str.toStdString(), mat3);
        QString mat4Str = *imageOutputFolder + "/mat4.png";
        imwrite(mat4Str.toStdString(), mat4);
        Mat draw;
        cvtColor(grayMat, draw, COLOR_GRAY2BGR);
        drawKeypoints(draw, keyPoints, draw, Scalar(0, 0, 255), DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
        Scalar color = Scalar(0, 255, 255);
        char buffer[128];
        for (uint i = 0; i < keyPoints.size(); i++) {
            sprintf(buffer, "%d:%.2f", i+1, double(keyPoints[i].size));
            putText(draw, buffer,
                    keyPoints[i].pt + Point2f(10, -10),
                    cv::FONT_HERSHEY_SIMPLEX, 1, color, 2, 8);
        }
        QString keyPointsImageName = *imageOutputFolder + "/keyPointsImageName.png";
        imwrite(keyPointsImageName.toStdString(), draw);;
    }

    return dvimg;
}

bool cmp_size(std::vector<CircleCenter> a, std::vector<CircleCenter> b)
{
    return a.size() > b.size();
}

bool Parse2DDicomImage::detectIronPointInRoi(cv::Mat src_gray, std::vector<cv::KeyPoint> &pointsVect, bool isGrayMat)
{
    qDebug("=== detectAllIronPoint == 打印的太多，里面很多要删除掉");
    // TODO: 这个有问题
    if(!isGrayMat) {
        // 把原图像转化成灰度图像
        cv::cvtColor(src_gray, src_gray, cv::COLOR_BGR2GRAY);
    }

    // 平滑处理
    cv::blur(src_gray, src_gray, cv::Size(3, 3));
    cv::Mat image_binary;
    std::vector< std::vector< cv::Point> > contours;
    std::vector< std::vector< cv::Point> > contoursbak;
    for(int thresh = 100; thresh > 50; thresh -=5) {
        contours.clear();

        cv::threshold(src_gray, image_binary, thresh, 255, cv::THRESH_BINARY);
        //QString filename = "/home/tiger/Log/ss" + QString::number(thresh) + ".png";
        //cv::imwrite(filename.toStdString().c_str(), image_binary);

        cv::findContours(
                    image_binary,
                    contours,
                    cv::noArray(),
                    cv::RETR_LIST,
                    cv::CHAIN_APPROX_SIMPLE
                    );

        std::vector< std::vector<cv::Point> >::iterator it= contours.begin();
        for (it = contours.begin(); it != contours.end();) {
            contoursbak.push_back(*it);

            double area = cv::contourArea(*it);
            cv::RotatedRect rect = cv::minAreaRect(*it);

            if(cv::pointPolygonTest(*it, rect.center, true) <= 0) {
                // 点在轮廓外
                contours.erase(it);
            } else {
                double rate = rect.size.height * 1.0 / rect.size.width;
                if(rate > 1.1 || rate < 0.9) {
                    contours.erase(it);
                } else {
                    if (it->size() > 200 || it->size() < 20
                            || area < 100 || area > 2500) {
                        contours.erase(it);
                    } else {
                        double areaRate = area / (rect.size.height * rect.size.width);
                        if(areaRate > 0.82 || areaRate < 0.74 ) {
                            contours.erase(it);
                        } else {
                            it++;
                        }
                    }
                }
            }
        }

        if(contours.size() == 1) {
            cv::RotatedRect contourRect = cv::minAreaRect(contours[0]);

            cv::KeyPoint point;
            point.pt.x = contourRect.center.x;
            point.pt.y = contourRect.center.y;
            point.size = contourRect.size.width;
            pointsVect.push_back(point);

            return true;
        }

        // 鼠标点击模式，且没有直接找到圆。考虑圆重叠
        if(!isGrayMat && contours.size() == 0) {
            qDebug() << "contoursbak.begin()" << contoursbak.size();
            for (it = contoursbak.begin(); it != contoursbak.end(); it++) {
                double area = cv::contourArea(*it);

                std::vector< std::vector<CircleCenter> > centerVector;
                if(area < 3500) {
                    qDebug() << "size:" << it->size();

                    for(int i = 0; i < it->size() - 10; i+= 5) {
                        CircleCenter circleCenter;
                        calCircle(it->at(i), it->at(i+5), it->at(i+10), circleCenter);

                        bool toAdd = true;
                        for(int j = 0; j < centerVector.size(); j++) {
                            if(distance2(centerVector.at(j).at(0), circleCenter) < 500) {
                                centerVector.at(j).push_back(circleCenter);
                                toAdd = false;
                                break;
                            }
                        }
                        if(toAdd) {
                            std::vector<CircleCenter> vect;
                            vect.push_back(circleCenter);
                            centerVector.push_back(vect);
                        }
                    }
                }

                qDebug() << centerVector.size();
                sort(centerVector.begin(), centerVector.end(), cmp_size);
                qDebug() << centerVector.size();
                if(centerVector.size() > 1 && centerVector.at(1).size() > 2) {
                    cv::KeyPoint point1;
                    for(int i = 0; i < centerVector.at(0).size(); i++) {
                        CircleCenter c = centerVector.at(0).at(i);
                        point1.pt.x += c.x;
                        point1.pt.y += c.y;
                        point1.size += c.r;
                    }

                    point1.pt.x /= centerVector.at(0).size();
                    point1.pt.y /= centerVector.at(0).size();
                    point1.size /= centerVector.at(0).size();
                    pointsVect.push_back(point1);


                    cv::KeyPoint point2;
                    for(int i = 0; i < centerVector.at(1).size(); i++) {
                        CircleCenter c = centerVector.at(1).at(i);
                        point2.pt.x += c.x;
                        point2.pt.y += c.y;
                        point2.size += c.r;
                    }

                    point2.pt.x /= centerVector.at(1).size();
                    point2.pt.y /= centerVector.at(1).size();
                    point2.size /= centerVector.at(1).size();
                    pointsVect.push_back(point2);

                    return true;
                }
            }
        }
    }
    return false;
}

// 已知圆上三点。求圆心，半径
void Parse2DDicomImage::calCircle(cv::Point pt1, cv::Point pt2, cv::Point pt3, CircleCenter &center)
{
    double x1 = pt1.x, x2 = pt2.x, x3 = pt3.x;
    double y1 = pt1.y, y2 = pt2.y, y3 = pt3.y;
    double a = x1 - x2;
    double b = y1 - y2;
    double c = x1 - x3;
    double d = y1 - y3;
    double e = ((x1 * x1 - x2 * x2) + (y1 * y1 - y2 * y2)) / 2.0;
    double f = ((x1 * x1 - x3 * x3) + (y1 * y1 - y3 * y3)) / 2.0;
    double det = b * c - a * d;
    if( fabs(det) < 1e-5)
    {
        center.r = -1;
        center.x = 0;
        center.y = 0;
        return;
    }

    center.x = -(d * e - b * f) / det;
    center.y = -(a * f - c * e) / det;
    center.r = hypot(x1 - center.x, y1 - center.y);
    return;
}

bool Parse2DDicomImage::getPointRect(int w, int h, int x, int y, Rect &rect, int l)
{
    if (x < 0 || y< 0 || x >= w || y >=h) {
        return false;
    }

    int r = 100/2;
    rect.x = x - r >= 0 ? x-r:0;
    rect.y = y - r >= 0 ? y-r:0;
    rect.width = (x+l < w) ? l:(w-x-1);
    rect.height = (y+l < h) ? l:(h-y-1);

    return true;
}

